#include <btl/ViewGeometry/EssentialMatrix.hpp>
#include <btl/Maths/FloatComparison.hpp>

#include <Eigen/SVD>
#include <cassert>

namespace btl
{
namespace viewgeometry
{

void DecomposeEssentialSVD::compute(const Eigen::Matrix3d& E, int opts)
{
   Eigen::JacobiSVD<Eigen::Matrix3d> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);

   // check that it really is an essential matrix
   const Eigen::Vector3d sv = svd.singularValues();

   // FIXME:
   //   maybe make this an option or make the tolerance value a parameter,
   //   so that DecomposeEssentialSVD can be used to fix-up slightly broken
   //   essential matrices.

   //assert(btl::almost_equal(sv(0), sv(1)));
   //assert(btl::almost_zero(sv(2)));

   const Eigen::Matrix3d W =
      (Eigen::Matrix3d()
         << 0.0, -1.0, 0.0,
            1.0,  0.0, 0.0,
            0.0,  0.0, 1.0
      ).finished();

   _t = -svd.matrixU().col(2);

   if ((opts & DecomposeEssentialSVD::ComputeNormalisedT) == 0)
      _t *= ((sv(0) + sv(1)) / 2.0);

   if (opts & DecomposeEssentialSVD::ComputeNormalisedE)
      _Enorm = svd.matrixU() * Eigen::Vector3d(1.0, 1.0, 0.0).asDiagonal() * svd.matrixV().transpose();
   else
      _Enorm = E;

   _Rx = -(svd.matrixU() * W * svd.matrixV().transpose());
   _Ry = -(svd.matrixU() * W.transpose() * svd.matrixV().transpose());
}

void DecomposeEssentialHorn::compute(const Eigen::Matrix3d& E, int opts)
{
   const Eigen::Matrix3d EEt = E * E.transpose();
   const double traceEEt_2 = 0.5 * EEt.trace();

   const double bSqr[3] = {
      traceEEt_2 - EEt(0,0),
      traceEEt_2 - EEt(1,1),
      traceEEt_2 - EEt(2,2)
   };

   int max;
   if (bSqr[2] > bSqr[1]) {
      max = (bSqr[2] > bSqr[0] ? 2 : 0);
   } else {
      max = (bSqr[1] > bSqr[0] ? 1 : 0);
   }

   const double bMax = std::sqrt(bSqr[max]);
   const double invMax = 1.0 / bMax;
   if (max == 0)
      _t = Eigen::Vector3d(bMax, invMax * -EEt(1,0), invMax * -EEt(2,0));
   else if (max == 1)
      _t = Eigen::Vector3d(invMax * -EEt(0,1), bMax, invMax * -EEt(2,1));
   else if (max == 2)
      _t = Eigen::Vector3d(invMax * -EEt(0,2), invMax * -EEt(1,2), bMax);
   else
      assert(0);

   const Eigen::Matrix3d cofactorsT = matrix3_cofactors(E).transpose();
   const Eigen::Matrix3d BE = cross_matrix(_t) * E;
   const double invMagSqr = (1.0 / (bSqr[0] + bSqr[1] + bSqr[2]));
   _Rplus = invMagSqr * (cofactorsT - BE);
   _Rminus = invMagSqr * (cofactorsT + BE);

   if (opts & DecomposeEssentialSVD::ComputeNormalisedE)
      _Enorm = cross_matrix(_t) * _Rplus;
   else
      _Enorm = E;

   if (opts & DecomposeEssentialSVD::ComputeNormalisedT)
      _t.normalize();
}

} // namespace viewgeometry
} // namespace btl
